import cv2
import rclpy
from cv_bridge import CvBridge
from rclpy.node import Node
from sensor_msgs.msg import Image

device = '/dev/video0'
width = 1920
height = 1080
framerate = 30


class ImagePublisher(Node):
    def __init__(self):
        super().__init__('image_publisher')

        # Create a publisher. This publisher will publish an Image to the video_frame topic.
        # The queue size is 0 messages.
        self.publisher_ = self.create_publisher(Image, 'video_frames', 0)

        # Publish a message every 0.033 seconds (fps 30)
        timer_period = round(1 / framerate, 3)

        # Create the timer
        self.timer = self.create_timer(timer_period, self.timer_callback)

        # Create a VideoCapture object
        camera = f'v4l2src device={device} ! video/x-raw,format=YUY2,width={width},' \
                 f'height={height},framerate={framerate}/1 ! videoconvert ! video/x-raw, ' \
                 f'format=BGR ! appsink drop=1'
        self.cap = cv2.VideoCapture(camera, cv2.CAP_GSTREAMER)

        # Used to convert between ROS and OpenCV images
        self.br = CvBridge()

    def timer_callback(self):
        # Capture frame-by-frame
        ret, frame = self.cap.read()

        if ret:
            # Publish the image. The `cv2_toimgmsg` method converts an OpenCV image to a ROS2 image message.
            self.publisher_.publish(self.br.cv2_to_imgmsg(frame))

        # Display the message on the console
        self.get_logger().info('Publishing video frame')


def main(args=None):
    # Initialize the rclpy library
    rclpy.init(args=args)

    # Create the node
    image_publisher = ImagePublisher()

    # Spin the node so the callback function is called.
    rclpy.spin(image_publisher)

    # Destroy the node explicitly (optional - otherwise it will be done automatically when the garbage collector
    # destroys the node object)
    image_publisher.destroy_node()

    # Shutdown the ROS2 client library for Python
    rclpy.shutdown()


if __name__ == '__main__':
    main()
